#include "AbandonObjectDetect.h"
#include "AODAlgo.h"
#include <future>
#include <thread>
using namespace std;

#include "CallOnce.h"
#include "Frequency.h"

cv::Mat CheckMat(cv::Mat mat, cv::Rect rtTarget) {
  if (rtTarget.x < 0) {
    rtTarget.x = 0;
  }
  if (rtTarget.y < 0) {
    rtTarget.y = 0;
  }
  if (rtTarget.width < 0) {
    rtTarget.width = 0;
  }
  if (rtTarget.height < 0) {
    rtTarget.height = 0;
  }
  if (rtTarget.x + rtTarget.width > mat.cols) {
    rtTarget.width = mat.cols - rtTarget.x;
  }
  if (rtTarget.y + rtTarget.height > mat.rows) {
    rtTarget.height = mat.rows - rtTarget.y;
  }

  assert(rtTarget.x >= 0);
  assert(rtTarget.y >= 0);
  assert(rtTarget.width >= 0);
  assert(rtTarget.height >= 0);
  assert(rtTarget.x + rtTarget.width <= mat.cols);
  assert(rtTarget.y + rtTarget.height <= mat.rows);

  return mat(rtTarget);
}

cv::Point getCenterPoint(cv::Rect rect) {
  cv::Point cpt;
  cpt.x = rect.x + cvRound(rect.width / 2.0);
  cpt.y = rect.y + cvRound(rect.height / 2.0);
  return cpt;
}

cv::Rect scale_rect(const cv::Rect &r, float scale) {
  cv::Point m = getCenterPoint(r);
  float width = r.width * scale;
  float height = r.height * scale;
  int x = cvRound(m.x - width / 2);
  int y = cvRound(m.y - height / 2);
  return cv::Rect(x, y, cvRound(width), cvRound(height));
}

//  检测是遗留还是移除 返回2表示遗留，1表示移除
int detectLeftRemove(const cv::Mat &background, cv::Rect &rect) {
  int returnVal = -1;

  // 扩展rect
  cv::Rect surroundRect = scale_rect(rect, 4);
  cv::Scalar S1 = mean(background(rect));
  auto background_surroundRect = CheckMat(background, surroundRect);
  cv::Scalar S2 = mean(background_surroundRect);
  double dist1 = norm(S1);
  double dist2 = norm(S2);
  double ratio = 0.0;
  if (dist1 >= dist2)
    ratio = dist2 / dist1;
  else
    ratio = dist1 / dist2;

  cout << "ratio:" << ratio << endl;
  if (ratio >= 0.85 && ratio <= 1.0) // 相近表示离开（ Remove ）
    returnVal = 1;
  else // 否则表示遗留（Left）
    returnVal = 2;

  return returnVal;
}

class AbandonObjectDetectPrivate {
public:
  AbandonObjectDetectPrivate(CallBack cb) { this->cb = cb; };

  ~AbandonObjectDetectPrivate(){};

  AODAlgo aa1;
  AODAlgo aa2;

  // 计算最大频率
  Frequency freq;

  CallOncePtr co;

  CallBack cb;

  // cv::Rect rtPre;

  // 判断是否连续的在遗留或丢失状态中
  bool bContinueous = false;
  int nFrameCountInState = 0;

  // frame count
  int nFrameCount = 0;

  // area threshold
  int nArea = 300;
  int nAreaMax = 20000000;
  // time threshold
  float fSec = 10.f;
  // reset count
  int nCount = 500;

  bool bUseSimpleTactics = true;
};

AbandonObjectDetect::AbandonObjectDetect(CallBack cb)
    : m_pHandle(new AbandonObjectDetectPrivate(cb)) {}

AbandonObjectDetect::~AbandonObjectDetect() {}

void AbandonObjectDetect::setROI(cv::Rect rt) {
  m_pHandle->aa1.setROI(rt);
  m_pHandle->aa2.setROI(rt);
};

void AbandonObjectDetect::setTimeThreshold(float fSec) {
  m_pHandle->fSec = fSec;
  m_pHandle->co = make_shared<CallOnce>(m_pHandle->fSec, m_pHandle->cb);
};
void AbandonObjectDetect::setAreaThreshold(int nSize) {
  m_pHandle->nArea = nSize;
};
void AbandonObjectDetect::setAreaThresholdMax(int nSize) {
  m_pHandle->nAreaMax = nSize;
};
void AbandonObjectDetect::setNeedToResetNumber(int nCount) {
  m_pHandle->nCount = nCount;
};
void AbandonObjectDetect::setTacticsFlag(bool bFlag) {
  m_pHandle->bUseSimpleTactics = bFlag;
};

void AbandonObjectDetect::detect(const AODInput &input) {
  // ros::Time t = ros::Time::now();
  // 异步并行算法
  std::vector<std::future<bool>> algo_aod;

  AODResult res1;
  AODResult res2;

  auto func1 = [&]() -> bool { m_pHandle->aa1.detect(input, res1); };
  algo_aod.push_back(std::async(std::launch::async, func1));

  auto func2 = [&]() -> bool { m_pHandle->aa2.detect(input, res2); };
  algo_aod.push_back(std::async(std::launch::async, func2));

  auto result = algo_aod[0].get() && algo_aod[1].get();

  // AODResult res1;
  // m_pHandle->aa1.detect(input, res1);
  // AODResult res2;
  // m_pHandle->aa2.detect(input, res2);

  cv::Rect rtMax = res1.rt.area() > res2.rt.area() ? res1.rt : res2.rt;

  // m_pHandle->co->publish(type);

  // 以下代码得到 一段时间内 出现频率最大的数据
  FrequencyResult freqResult;

  if (m_pHandle->bUseSimpleTactics) {
    if (rtMax.area() > m_pHandle->nArea && rtMax.area() < m_pHandle->nAreaMax) {
      freqResult.rt = rtMax;
    }
  } else {
    // 区域大于阈值
    if (rtMax.area() > m_pHandle->nArea) {
      m_pHandle->bContinueous = true;

      FrequencyInput freqInput;
      freqInput.rt = rtMax;
      freqResult = m_pHandle->freq.detectFrequency(freqInput);
    } else {
      // 如果刚退出continuous，需要强制检查frenquency，并且重置
      if (m_pHandle->bContinueous) {
        m_pHandle->bContinueous = false;
        freqResult = m_pHandle->freq.detectFrequencyForce();
      }
    }
  }

  // algo need to reset for every fix frame
  m_pHandle->nFrameCount++;
  if (m_pHandle->nFrameCount == m_pHandle->nCount) {
    m_pHandle->nFrameCount = 0;
    m_pHandle->aa2.reset();
  }

  if (m_pHandle->nFrameCount == m_pHandle->nCount / 2) {
    m_pHandle->aa1.reset();
  }

  if (freqResult.rt.area() > 0) {
    int type = detectLeftRemove(input.mat, freqResult.rt);

    MsgAOD msg;
    msg.type = type;
    msg.rt = freqResult.rt;
    m_pHandle->co->publish(msg);
  }
};
